/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * utilities.cxx
 *
 * PURPOSE:
 *   System utilities functions
 *
 * CREATED:
 * 2/2009 by Cory Dixon
 *
 ***********************************************************************/

#include <dirent.h>
#include "utilities.h"

//-------------------- Data Filtering ----------------------------//
// alpha is smoothing factor
float smoothed(float past, float measure, float alpha)
{
  return (1-alpha) * past + (alpha) * measure;
}

//-- Digital Butterworth 5th order filter 
float butter_filter_5(float val, float xv[BUTTER_NZEROS+1], float yv[BUTTER_NPOLES+1])
{ 
	// time shift
	for( int i=0; i < BUTTER_NZEROS; i++)  xv[i] = xv[i+1]; 
	for( int i=0; i < BUTTER_NPOLES; i++)  yv[i] = yv[i+1]; 

	// new values
	xv[5] = val / double(BUTTER_GAIN);
	if( IsNaN(xv[0]) ) {
		yv[5] = val;
	} else {
		yv[5] =  (xv[0] + xv[5]) + 5 * (xv[1] + xv[4]) + 10 * (xv[2] + xv[3])
			+ (  0.1254306222 * yv[0]) + ( -0.8811300754 * yv[1])
			+ (  2.5452528683 * yv[2]) + ( -3.8060181193 * yv[3])
			+ (  2.9754221097 * yv[4]);
	}
	// return filtered value
	return yv[5];
}


int Round(double Value)
{
	int Return = (int)Value;
	double Rem;

	// Calculate the difference between the true number and the truncated
	//   value.  If Value is positive this will be positive.  If Value is
	//   negative this will be negative
	Rem = Value - Return;
	if(Rem > 0.5)
		Return++;
	else if(Rem < -0.5)
		Return--;

	return Return;
}


//-------------------- File I/O ----------------------------//
String base_file;

// file select is called by scandir and should not be called directly
int fileSelect(const struct dirent *entry)
{
  if (strncmp(entry->d_name, base_file.c_str(), base_file.length()) == 0)
    return true;

 return false;
}

// find open numbered file
String generteNumberedFile( String dir, String base, String ext )
{
	struct dirent **files;
	base_file = base;
	char log_name[1024];

	int num = scandir(dir.c_str(), &files, fileSelect, NULL);
	if (num < 0) {
		printf("Unable to open file directory: %s\n", dir.c_str());
		return 0;
	} 
	sprintf(log_name,"%s%s%03d.%s", dir.c_str(), base.c_str(), num, ext.c_str());
	//printf("using %s\n",log_name);

	// clean up
	while (num--) {free(files[num]);}
	free(files);

	return log_name;
}

//-------------------- GPS <-> range ----------------------------//
// Calculate the distance [m] between two gps lat/lon coordinates, note
// this assumes a flat earth model.
double GPSRange(GPSLLA_t pos1, GPSLLA_t pos2)
{
	double d_lat   = pos1.lat - pos2.lat;
	double d_lon   = pos1.lon - pos2.lon;
	double d_alt   = pos1.alt - pos2.alt;
	double lat_ref = pos1.lat;
	double d_x     = LAT_TO_M(d_lat);
	double d_y     = LON_TO_M(d_lon, lat_ref);

	return sqrt(d_x*d_x + d_y*d_y + d_alt*d_alt);
}

//-------------------- GPS <-> distance ----------------------------//
// Calculate the distance [m] between two gps lat/lon coordinates, note
// this assumes a flat earth model and no altitude seperation.
double GPSDist(GPSLLA_t pos1, GPSLLA_t pos2)
{
	double d_lat   = pos1.lat - pos2.lat;
	double d_lon   = pos1.lon - pos2.lon;
	double lat_ref = pos1.lat;
	double d_x     = LAT_TO_M(d_lat);
	double d_y     = LON_TO_M(d_lon, lat_ref);

	return sqrt(d_x*d_x + d_y*d_y);
}


//-------------------- GPS <-> bearing ----------------------------//
// Calculate the bearing [deg] between two gps lat/lon coordinates
double GPSBearing(GPSLLA_t pos1, GPSLLA_t pos2)
{
	double r_lat1  = pos1.lat*DEG_2_RAD;
	double r_lat2  = pos2.lat*DEG_2_RAD;
	double r_lon1  = pos1.lon*DEG_2_RAD;
	double r_lon2  = pos2.lon*DEG_2_RAD;

	return fmod( atan2( sin(r_lon2-r_lon1)*cos(r_lat2), cos(r_lat1)*sin(r_lat2)-sin(r_lat1)*cos(r_lat2)*cos(r_lon2-r_lon1)), 2*M_PI ) * RAD_2_DEG;

}

// ECEF2LLA - convert earth-centered earth-fixed (ECEF)
//            cartesian coordinates to latitude, longitude,
//            and altitude
// 
//  CartVector_t:
//      x = ECEF X-coordinate (m)
//      y = ECEF Y-coordinate (m)
//      z = ECEF Z-coordinate (m)
//  GPSLLA_t:
//      lat = geodetic latitude (deg)
//      lon = longitude (deg)
//      alt = height above WGS84 ellipsoid (m)
// 
// Notes: 1) This function assumes the WGS84 model.
//        2) Latitude is customary geodetic (not geocentric).
GPSLLA_t ecef2lla(CartVector_t &ecef)
{
	GPSLLA_t gps;
	
	// WGS84 ellipsoid constants:
	double a   = 6378137.0l;
	double e2  = 6.6943799901414e-3l;
	double b   = 6.356752314245179e+06l; //sqrt(a^2*(1-e2));
	double ep2 = 6.739496742276433e-03l; //ep*ep, ep = sqrt((a^2-b^2)/b^2);

	// calculations:
	double x2  = ecef.x*ecef.x;
	double y2  = ecef.y*ecef.y;

	double p   = sqrt(x2+y2);
	double th  = atan2(a*ecef.z, b*p);
	double lon = atan2(ecef.y, ecef.x);
	double lat = atan2((ecef.z+ep2*b*pow(sin(th),3)),(p-e2*a*pow(cos(th),3)));
	double N   = a/sqrt(1-e2*pow(sin(lat),2));
	double alt = p/cos(lat)-N;

	// return lon in range [0,2*pi)
	lon = fmod(lon, PI_2);

	// correct for numerical instability in altitude near exact poles:
	// (after this correction, error is about 2 millimeters, which is about
	// the same as the numerical precision of the overall function)
	if( fabs(ecef.x) < 1.0 && fabs(ecef.y) < 1.0 )
		alt = fabs(ecef.z) - b;

	// now fill in return variables, with correct units
	//    [rad]->[deg], [m]->[m]
	gps.lat = lat * RAD_2_DEG;
	gps.lon = lon * RAD_2_DEG;
	gps.alt = alt;

	return gps;
}

Matrix ecef2lla(Matrix &ecef)
{
	Matrix lla(3,1);
	
	// WGS84 ellipsoid constants:
	double a   = 6378137.0l;
	double e2  = 6.6943799901414e-3l;
	double b   = 6.356752314245179e+06l; //sqrt(a^2*(1-e2));
	double ep2 = 6.739496742276433e-03l; //ep*ep, ep = sqrt((a^2-b^2)/b^2);

	// calculations:
	double x2  = ecef(0)*ecef(0);
	double y2  = ecef(1)*ecef(1);

	double p   = sqrt(x2+y2);
	double th  = atan2(a*ecef(2), b*p);
	double lon = atan2(ecef(1), ecef(0));
	double lat = atan2((ecef(2)+ep2*b*pow(sin(th),3)),(p-e2*a*pow(cos(th),3)));
	double N   = a/sqrt(1-e2*pow(sin(lat),2));
	double alt = p/cos(lat)-N;

	// return lon in range [0,2*pi)
	lon = fmod(lon, PI_2);

	// correct for numerical instability in altitude near exact poles:
	// (after this correction, error is about 2 millimeters, which is about
	// the same as the numerical precision of the overall function)
	if( fabs(ecef(0)) < 1.0 && fabs(ecef(1)) < 1.0 )
		alt = fabs(ecef(2)) - b;

	// now fill in return variables, with correct units
	//    [rad]->[deg], [m]->[m]
	lla(0) = lat * RAD_2_DEG;
	lla(1) = lon * RAD_2_DEG;
	lla(2) = alt;

	return lla;
}

// LLA2ECEF - convert latitude, longitude, and altitude to
//            earth-centered, earth-fixed (ECEF) cartesian
// 
// USAGE:
// CartVector_t = lla2ecef(GPSLLA_t)
//  
//  CartVector_t:
//      x = ECEF X-coordinate (m)
//      y = ECEF Y-coordinate (m)
//      z = ECEF Z-coordinate (m)
//  GPSLLA_t:
//      lat = geodetic latitude (deg)
//      lon = longitude (deg)
//      alt = height above WGS84 ellipsoid (m)
// 
// Notes: 1) This function assumes the WGS84 model.
//        2) Latitude is customary geodetic (not geocentric).
CartVector_t lla2ecef(GPSLLA_t &gps)
{
	CartVector_t ecef;
	
	// WGS84 ellipsoid constants:
	double a = 6378137.0l;
	double e2 = 6.6943799901414e-3l;

	// do unit conversion [deg]->[rad], [m]->[m]
	double lat = gps.lat * DEG_2_RAD;
	double lon = gps.lon * DEG_2_RAD;
	double alt = gps.alt;

	// intermediate calculation (prime vertical radius of curvature)
	double s_lat = sin(lat);
	double N = a / sqrt(1 - e2 * s_lat*s_lat);

	// results:
	ecef.x = (N+alt) * cos(lat) * cos(lon);
	ecef.y = (N+alt) * cos(lat) * sin(lon);
	ecef.z = ((1-e2) * N + alt) * sin(lat);

	return ecef;
}

// LLA2LOCAL - convert latitude, longitude, and altitude to a local coordinate frame
// 
// USAGE:
// CartVector_t = lla2local(GPSLLA_t, GPSLLA_t)
//  
//  CartVector_t:
//      x = LOCAL X-coordinate (m)
//      y = LOCAL Y-coordinate (m)
//      z = Altitude AGL  (m)
//  GPSLLA_t:
//      lat = geodetic latitude (deg)
//      lon = longitude (deg)
//      alt = height above WGS84 ellipsoid (m)
CartVector_t lla2local(GPSLLA_t &gps, GPSLLA_t &datum)
{
	CartVector_t local;
	
	// WGS84 ellipsoid constants:
	double Re = 6378137.0l;

	// do unit conversion [deg]->[rad], [m]->[m]
  double d_lat = (gps.lat - datum.lat) * DEG_2_RAD;
  double d_lon = (gps.lon - datum.lon) * DEG_2_RAD;

  double r = Re*2*sqrt( (d_lat*d_lat/4) + cos(datum.lat*DEG_2_RAD)*cos(gps.lat*DEG_2_RAD)*(d_lon*d_lon/4) );
  double th = atan2( sin(d_lon)*cos(gps.lat*DEG_2_RAD), cos(datum.lat*DEG_2_RAD)*sin(gps.lat*DEG_2_RAD) - sin(datum.lat*DEG_2_RAD)*cos(gps.lat*DEG_2_RAD)*cos(d_lon) );

  // position of gps in cylindrical

	// results:
	local.x = r * cos(th);
	local.y = r * sin(th);
	local.z = gps.alt - datum.alt;

	return local;
}

Matrix lla2ecef(Matrix &lla)
{
	Matrix ecef(3,1);
	
	// WGS84 ellipsoid constants:
	double a = 6378137.0l;
	double e2 = 6.6943799901414e-3l;

	// do unit conversion [deg]->[rad], [m]->[m]
	double lat = lla(0) * DEG_2_RAD;
	double lon = lla(1) * DEG_2_RAD;
	double alt = lla(2);

	// intermediate calculation (prime vertical radius of curvature)
	double s_lat = sin(lat);
	double N = a / sqrt(1 - e2 * s_lat*s_lat);

	// results:
	ecef(0) = (N+alt) * cos(lat) * cos(lon);
	ecef(1) = (N+alt) * cos(lat) * sin(lon);
	ecef(2) = ((1-e2) * N + alt) * sin(lat);

	return ecef;
}

// Calculates the direction cosine matrix from ECEF to NED
// given the geodectic latitude and longitude in deg
//
// Cory Dixon
// Feb. 4, 2009
Matrix ecef2nedDCM(GPSLLA_t lla)
{
	Matrix m_lla(3,1);
	m_lla(0) = lla.lat;
	m_lla(1) = lla.lon;
	m_lla(2) = lla.alt;
	return ecef2nedDCM(m_lla);
}

Matrix ecef2nedDCM(Matrix &lla)
{
	double lat = lla(0) * DEG_2_RAD;
	double lon = lla(1) * DEG_2_RAD;

	double t1[9] = {0, 0, 1, 0, 1, 0, -1, 0, 0};
	double t2[9] = {cos(lat), 0, sin(lat), 0, 1, 0, -sin(lat), 0, cos(lat)};
	double t3[9] = {cos(lon), sin(lon), 0, -sin(lon), cos(lon), 0, 0, 0, 1};

	Matrix T1(3,3,t1);
	Matrix T2(3,3,t2);
	Matrix T3(3,3,t3);

	return (T1*T2*T3);
}

//-------------------- Orbit Controller ----------------------------//
void vectorField(GPSLLA_t vehicle, float vehicle_heading, GPSLLA_t target, 
                 float rdes, float speed, Velocity_t* vdes, float* psides, float* psidotdes)
{
	GPSLLA_t delx;

	// get difference in locations
	delx.lat = LAT_TO_M(vehicle.lat - target.lat);
	delx.lon = LON_TO_M(vehicle.lon - target.lon, vehicle.lat);
	float r = sqrt(delx.lat*delx.lat + delx.lon*delx.lon);

	if (r==0) {
		vdes->x = speed*(cos(vehicle_heading));
		vdes->y = speed*(sin(vehicle_heading));
		*psides = vehicle_heading;
		*psidotdes = 0;
		return;
	}

	float den   = r*r+rdes*rdes;
	float num1  = r*r-rdes*rdes;
	float num2  = 2*r*rdes;
	float frac1 = num1/den;
	float frac2 = num2/den;

	vdes->x = (speed/r)*(-delx.lat*frac1-delx.lon*frac2);
	vdes->y = (speed/r)*(-delx.lon*frac1+delx.lat*frac2);

	*psides    = (float)atan2(vdes->y, vdes->x);
	*psidotdes = 4*speed*rdes*r*r/(den * den);
}

float getTurnRate(float psi, float psides, float psidotdes, float k_p) 
{
	float dpsi = psi-psides;
	while (dpsi > M_PI) dpsi = dpsi-2*M_PI;
	while (dpsi < -M_PI) dpsi = dpsi+2*M_PI;

	return (-k_p*dpsi+psidotdes);
}

float getTurnRate(float psi,  float psidot, float psides, float k_p, float k_d) 
{
	float dpsi = psi-psides;
	while (dpsi > M_PI) dpsi = dpsi-2*M_PI;
	while (dpsi < -M_PI) dpsi = dpsi+2*M_PI;

	return (-k_p*dpsi-k_d*psidot);
}

float getVelocityRate(float phase, float partner_phase, float k_p) 
{
	float dphase = phase-partner_phase;
	while (dphase > 180) dphase = dphase-2*180;
	while (dphase < -180) dphase = dphase+2*180;

	cout << "dphase: " << dphase << " " << (dphase/fabs(dphase))*(fabs(dphase)-180) << " " << flush;

	float dspeed = (k_p*(dphase/fabs(dphase))*(fabs(dphase)-180));
	return dspeed;
}

string getFileName(const string &str)
{
	string file;
	size_t found;
	found=str.find_last_of("/\\");

	if( found >= 0 ) {
		file = str.substr(found+1);
	}
	return file;
}

/*
 * Description:
 *   Find and replace text within a string.
 *
 * Parameters:
 *   src  (in) - pointer to source string
 *   from (in) - pointer to search text
 *   to   (in) - pointer to replacement text
 * 
 * Returns:
 *   Returns a pointer to dynamically-allocated memory containing string
 *   with occurences of the text pointed to by 'from' replaced by with the
 *   text pointed to by 'to'.
 */
char * replace_string(const char *src, const char *from, const char *to)
{  
	size_t size    = strlen(src) + 1;
	size_t fromlen = strlen(from);
	size_t tolen   = strlen(to);

	/* Allocate the first chunk with enough for the original string. */
	char *value = (char *)malloc(size);


	/* We need to return 'value', so let's make a copy to mess around with. */
	char *dst = value;

	if ( value != NULL )
	{
		for ( ;; )
		{
			/* Try to find the search text. */
			const char *match = strstr(src, from);
			if ( match != NULL )
			{
				char *temp;
				/* Find out how many characters to copy up to the 'match'. */
				size_t count = match - src;


				/* Calculate the total size the string will be after the
				 * replacement is performed. */
				size += tolen - fromlen;

				temp = (char *)realloc(value, size);
				if ( temp == NULL )
				{
					free(value);
					return NULL;
				}

				/* we'll want to return 'value' eventually, so let's point it 
				 * to the memory that we are now working with. 
				 * And let's not forget to point to the right location in 
				 * the destination as well. */
				dst = temp + (dst - value);
				value = temp;

				/*
				 * Copy from the source to the point where we matched. Then
				 * move the source pointer ahead by the amount we copied. And
				 * move the destination pointer ahead by the same amount.
				 */
				memmove(dst, src, count);
				src += count;
				dst += count;

				/* Now copy in the replacement text 'to' at the position of
				 * the match. Adjust the source pointer by the text we replaced.
				 * Adjust the destination pointer by the amount of replacement
				 * text. */
				memmove(dst, to, tolen);
				src += fromlen;
				dst += tolen;
			}
			else
			{
				/*
				 * Copy any remaining part of the string. This includes the null
				 * termination character.
				 */
				strcpy(dst, src);
				break;
			}
		}
	}
	return value;
}

int getdir (string dir, vector<string> &files)
{
	DIR *dp;
	struct dirent *dirp;
	if((dp  = opendir(dir.c_str())) == NULL) {
		cout << "Error(" << errno << ") opening " << dir << endl;
		return errno;
	}

	while ((dirp = readdir(dp)) != NULL) {
		files.push_back(string(dirp->d_name));
	}
	closedir(dp);
	return 0;
}

//-------------------- Units ----------------------------//
const char *lengthU_str(UnitSystem_t unit, double range)
{
	switch( unit ) {
		case SI_M_U: 
			//if( IsNaN(range) || range < 3000 )
				return "m";
			//else
				//return "km";
			//break;
		case SI_KM_U: 
			return "km";
			break;
		case ENGLISH_F_U: // base unit of feet
			return "ft";
			break;
		case ENGLISH_M_U:  // base unit of mile
			return "mi";
			break;
		case ENGLISH_NM_U: // base unit of nautical mile
			return "nm";
			break;
	}
	return "";
}

const char *speedU_str(UnitSystem_t unit)
{
	switch( unit ) {
	case SI_M_U: 
		return "m/s";
		break;
	case SI_KM_U: 
		return "kph";
		break;
	case ENGLISH_F_U: // base unit of feet
		return "fps";
		break;
	case ENGLISH_M_U:  // base unit of mile
		return "mph";
		break;
	case ENGLISH_NM_U: // base unit of nautical mile
		return "knots";
		break;
	}
	return "";
}

double meterToLengthU(double m, UnitSystem_t unit) {
	switch( unit ) {
		case SI_M_U:       // base unit of meters
			return m;
			break;
		case SI_KM_U:      // base unit of kilo-meters
			return m / 1000.0;
			break;
		case ENGLISH_F_U:  // base unit of feet
			return m * METERS_TO_FEET;
			break;
		case ENGLISH_M_U:  // base unit of mile
			return m * METERS_TO_MILES;
			break;
		case ENGLISH_NM_U: // base unit of nautical mile
			return m * METERS_TO_NMILES;
			break;
		default:
			return m;
	}
}
double feetToLengthU(double m, UnitSystem_t unit) {
	switch( unit ) {
		case SI_M_U:       // base unit of meters
			return m * FEET_TO_METERS;
			break;
		case SI_KM_U:      // base unit of kilo-meters
			return m * FEET_TO_METERS / 1000.0;
			break;
		case ENGLISH_F_U:  // base unit of feet
			return m;
			break;
		case ENGLISH_M_U:  // base unit of mile
			return m * FEET_TO_MILES;
			break;
		case ENGLISH_NM_U: // base unit of nautical mile
			return m * FEET_TO_NMILES;
			break;
		default:
			return m;
	}
}
double mpsToSpeedU(double mps, UnitSystem_t unit) {
	switch( unit ) {
		case SI_M_U:       // base unit of meters
			return mps;
			break;
		case SI_KM_U:      // base unit of kilo-meters
			return mps * MPS_TO_KPH;
			break;
		case ENGLISH_F_U:  // base unit of feet
			return mps * MPS_TO_FPS;
			break;
		case ENGLISH_M_U:  // base unit of mile
			return mps * MPS_TO_MPH;
			break;
		case ENGLISH_NM_U: // base unit of nautical mile
			return mps * MPS_TO_KNOTS;
			break;
		default:
			return mps;
	}
}
double mphToSpeedU(double m, UnitSystem_t unit) {
	switch( unit ) {
		case SI_M_U:       // base unit of meters
			return m * MPH_TO_MPS;
			break;
		case SI_KM_U:      // base unit of kilo-meters
			return m * MPH_TO_KPH;
			break;
		case ENGLISH_F_U:  // base unit of feet
			return m * MPH_TO_FPS;
			break;
		case ENGLISH_M_U:  // base unit of mile
			return m;
			break;
		case ENGLISH_NM_U: // base unit of nautical mile
			return m * MPH_TO_KNOTS;
			break;
		default:
			return m;
	}
}
double ktsToSpeedU(double m, UnitSystem_t unit) {
	switch( unit ) {
		case SI_M_U:       // base unit of meters
			return m * KNOTS_TO_MPS;
			break;
		case SI_KM_U:      // base unit of kilo-meters
			return m * KNOTS_TO_KPH;
			break;
		case ENGLISH_F_U:  // base unit of feet
			return m * KNOTS_TO_FPS;
			break;
		case ENGLISH_M_U:  // base unit of mile
			return m * KNOTS_TO_MPH;
			break;
		case ENGLISH_NM_U: // base unit of nautical mile
			return m;
			break;
		default:
			return m;
	}
}
